#include "sdcard.hpp"

#define CMD(x) (x)
#define ACMD(x) (x)

SDCard::SDCard(SDCardCallback *callback,SDCard::BusType bus){
    uint8_t R1;
    uint32_t retry;
    uint8_t buff[4];
    uint16_t n;
    this->busType = bus;
    this->callback = callback;

    if ( this->busType == BusType::Bus_SDIO ){
        /* 未实现SDIO */
        this->info.type = CardType::Unknown;
        return;
    }else if ( this->busType == BusType::Bus_SPI ){
        /* SPI初始化流程 */
        this->callback->busInit();
        this->callback->setSpeed(0);
        for ( n = 0; n < 10; n++ ) this->callback->readWriteByte(0xff); /* 发送至少74个脉冲 */
        retry = 20;
        do {
            R1 = this->sendCommand(CMD(0),0x00,0x95);
        }while ( (R1 != 0x01) && retry-- );
        this->info.type = CardType::Unknown;
        if ( R1 == 0x01 ){
            if ( this->sendCommand(CMD(8),0x01aa,0x87) == 0x01 ){
                /* 2.0的卡 */
                for ( n = 0; n < 4; n++ ) buff[n] = this->callback->readWriteByte(0xff);
                if ( buff[2] == 0x01 && buff[3] == 0xaa ){
                    retry = 0xfffe;
                    do{
                        this->sendCommand(CMD(55),0x00,0x01);
                        R1 = this->sendCommand(ACMD(41),0x40000000,0x01);  
                    }while ( R1 && retry-- );
                    if ( retry && this->sendCommand(ACMD(58),0x00,0x01) == 0x00 ){
                        for ( n = 0; n < 4; n++ ) buff[n] = this->callback->readWriteByte(0xff);
                        if ( buff[0] & 0x40 ) this->info.type = CardType::V2HC;
                        else this->info.type = CardType::V2;
                    }
                }
            }else{
                /* 1.0的卡 */
                this->sendCommand(CMD(55),0x00,0x01);
                R1 = this->sendCommand(ACMD(41),0x00,0x01);
                if ( R1 <= 0x01 ){
                    this->info.type = CardType::V1;
                    retry = 0xfffe;
                    do{
                        this->sendCommand(CMD(55),0x00,0x01);
                        R1 = this->sendCommand(ACMD(41),0x00,0x01);
                    }while ( R1 && retry-- );
                }else{
                    this->info.type = CardType::MMC;
                    retry = 0xfffe;
                    do{
                        R1 = this->sendCommand(CMD(1),0x00,0x01);
                    }while ( R1 && retry-- );
                }
                if ( retry == 0 || this->sendCommand(CMD(16),512,0x01) != 0x00 ){
                    this->info.type = CardType::Unknown;
                }
            }
        }
        this->callback->setSpeed(1);
        /* 获取卡CSD */
        if ( this->busType == BusType::Bus_SDIO ){
            /* 未实现 */
            this->info.capacity = 0;
            this->info.blockSize = 0;
        }else if ( this->busType == BusType::Bus_SPI ){
            /* 发送CMD9获取CSD */
            if ( (R1 = this->sendCommand(CMD(9),0x00,0x01)) == 0x00 ){
                retry = 200;
                /* 等待起始令牌 */
                while ( this->callback->readWriteByte(0xff) != 0xfe && retry-- );
                if ( retry != 0 ){
                    for ( n = 0; n < 16; n++ ){
                        this->csd[n] = (this->callback->readWriteByte(0xff) & 0xff);
                    }
                    /* CRC */
                    this->callback->readWriteByte(0xff);
                    this->callback->readWriteByte(0xff);
                    this->callback->setEnable(0);
                    this->callback->readWriteByte(0xff);
                }
            }
        }
        /* 计算卡容量 */
        if ( this->info.type == CardType::V2HC ){
            this->info.blockSize = 512;
            /* 借用retry计算DeviceSize */
            retry = 0;
            retry |= ( csd[7] & 0x3f ) << 16;
            retry |= (csd[8] & 0xff) << 8;
            retry |= (csd[9] & 0xff);
            this->info.capacity = (retry + 1) * 1024;
        }else if ( this->info.type == CardType::MMC | this->info.type == CardType::V1 | this->info.type == CardType::V2 ){
            uint8_t blockLen = csd[5] & 0x0f;
            uint8_t deviceSizeMul = ( csd[9] & 0x03 ) << 1;
            deviceSizeMul |= ( csd[10] & 0x80 ) >> 7;
            /* 借用retry存储DeviceSize */
            retry = ( csd[6] & 0x03 ) << 10;
            retry |= csd[7] << 2;
            retry |= ( csd[8] & 0xc0 ) >> 6;

            this->info.blockSize = 1 << blockLen;
            this->info.capacity = (retry + 1) * (0x01 << (deviceSizeMul + 2));   
            this->info.capacity *= this->info.blockSize / 512;
            this->info.blockSize = 512;
        }
        if ( this->info.type != CardType::Unknown ){
            FLAG_SET(this->flag,SDCard::Flag_Initialized);
        }
    }
}

uint8_t SDCard::writeBlock(uint8_t *buff,uint32_t startBlock,uint32_t count){
    uint8_t R1;
    uint16_t n;
    if ( this->busType == BusType::Bus_SDIO ){
        /* 未实现 */
        return 0;
    }else if ( this->busType == BusType::Bus_SPI ){
        /* 如果不是V2HC则需要拓展到字节地址 */
        if ( this->info.type != CardType::V2HC ) startBlock *= this->info.blockSize;
        if ( count == 1 ){
            /* 单块写 */
            R1 = this->sendCommand(CMD(24),startBlock,0x01);
            if ( R1 == 0x00 ){
                do{
                    R1 = this->callback->readWriteByte(0xff);
                }while ( R1 != 0xff );
                /* 写入起始Token */
                this->callback->readWriteByte(0xfe);
                for ( n = 0; n < this->info.blockSize; n++ ){
                    this->callback->readWriteByte(buff[n]);
                }
                /* 忽略掉CRC */
                this->callback->readWriteByte(0xff);
                this->callback->readWriteByte(0xff);
                R1 = this->callback->readWriteByte(0xff);
                if ( (R1 & 0x1f) != 0x05 ) {
                    return 0;
                }
                /* 发送CMD13检查写入结果 */
                if ( (R1 = this->sendCommand(CMD(13),0x00,0x01)) != 0x00 ) return 0;
                if ( (R1 = this->callback->readWriteByte(0xff)) != 0x00 ) return 0;
            }
        }else{
            /* 多块写 */
            this->sendCommand(CMD(55),0x00,0x01);
            this->sendCommand(ACMD(23),count,0x01);
            R1 = this->sendCommand(CMD(25),startBlock,0x01);
            if ( R1 == 0x00 ){
                for ( int pos = 0; pos < count; pos++ ){
                    do{
                        R1 = this->callback->readWriteByte(0xff);
                    }while ( R1 != 0xff );
                    /* 写入起始Token */
                    this->callback->readWriteByte(0xfe);
                    /* 写入数据 */
                    for ( n = 0; n < this->info.blockSize; n++ ) this->callback->readWriteByte(buff[n]);
                    /* 忽略CRC */
                    this->callback->readWriteByte(0xff);
                    this->callback->readWriteByte(0xff);
                    R1 = this->callback->readWriteByte(0xff);
                    if ( (R1 & 0x1f) != 0x05 ) return 0;
                    buff += this->info.blockSize;
                    /* 发送CMD13检查发送结果 */
                    if ( (R1 = this->sendCommand(CMD(13),0x00,0x01)) != 0x00 ) return 0;
                    if ( (R1 = this->callback->readWriteByte(0xff)) != 0x00 ) return 0;
                    if ( R1 != 0x00 ) break;
                }
                this->callback->readWriteByte(0xfd);
            }
        }
        this->callback->setEnable(0);
        return count;
    }
    return 0;
}

uint8_t SDCard::readBlock(uint8_t *buff,uint32_t startBlock,uint32_t count){
    uint8_t R1;
    uint16_t n,retry;
    if ( this->busType == BusType::Bus_SDIO ){
        /* 未实现 */
        return 0;
    }else if ( this->busType == BusType::Bus_SPI ){
        if ( this->info.type != CardType::V2HC ) startBlock *= this->info.blockSize;
        if ( count == 1 ){
            if ( (R1 = this->sendCommand(CMD(17),startBlock,0x01)) != 0x00 ) return 0;
            /* 等待起始Token */
            retry = 200;
            while ( this->callback->readWriteByte(0xff) != 0xfe && retry-- );
            if ( retry == 0 ) return 0;
            for ( n = 0; n < this->info.blockSize; n++ ){
                buff[n] = this->callback->readWriteByte(0xff);
            }
            /* 忽略CRC */
            this->callback->readWriteByte(0xff);
            this->callback->readWriteByte(0xff);
        }else{
            if ( (R1 = this->sendCommand(CMD(18),startBlock,0x01)) != 0x00 ) return 0;
            for ( int pos = 0; pos < count; pos++ ) {
                /* 等待起始Token */
                retry = 200;
                while ( this->callback->readWriteByte(0xff) != 0xfe && retry-- );
                if ( retry == 0 ) return 0;
                for ( n = 0; n < this->info.blockSize; n++ ){
                    buff[n] = this->callback->readWriteByte(0xff);
                }
                /* 忽略CRC */
                this->callback->readWriteByte(0xff);
                this->callback->readWriteByte(0xff);
                buff += this->info.blockSize;
            }
            this->sendCommand(CMD(12),0x00,0x01);
        }
        this->callback->setEnable(0);
        return count;
    }
    return 0;
}

SDCard::CardInfo SDCard::getInfo(void){
    return this->info;
}

uint8_t SDCard::sendCommand(uint8_t cmd,uint32_t arg,uint8_t crc){
    uint8_t R1;
    uint16_t retry;
    if ( this->busType == BusType::Bus_SDIO ){
        /* 未实现SDIO */
    }else if ( this->busType == BusType::Bus_SPI ){
        this->callback->setEnable(0);
        this->callback->readWriteByte(0xff);
        /* 片选 */
        this->callback->setEnable(1);
        this->callback->readWriteByte(cmd | 0x40);
        this->callback->readWriteByte(arg >> 24);
        this->callback->readWriteByte(arg >> 16);
        this->callback->readWriteByte(arg >> 8);
        this->callback->readWriteByte(arg);
        this->callback->readWriteByte(crc); 
        if ( cmd == CMD(12) ) this->callback->readWriteByte(0xff);
        retry = 0x1f;
        do{
            R1 = this->callback->readWriteByte(0xff);
        }while ( (R1 & 0x80) && retry-- );
        return R1;
    }
    return 0x00;
}
